/*
 * control.hpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */

#ifndef CONTROL_HPP_
#define CONTROL_HPP_

#ifdef __cplusplus
extern "C"
{
#endif

#include "math.h"
#ifdef __cplusplus
}
#endif


#ifdef __cplusplus

#include "userlib/userlib.hpp"
#include "pid/pid.hpp"

#define ROLL_PWM_REMOTE_MID 1500
#define PITCH_PWM_REMOTE_MID 1500
#define YAW_PWM_REMOTE_MID 1500
#define trim_roll_angle 2.5f
#define trim_pitch_angle -1.0f   //1.1
#define RAD2PWM 500.0f
#define ACCEL_ROLL_MAX 1100.0f
#define ACCEL_PITCH_MAX 1100.0f
#define ACCEL_YAW_MAX 270.0f
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN 360.0f
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX 720.0f
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN 90.0f
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX 360.0f
#define SMOOTHING_GAIN 7
#define CONTROL_DT 0.01f
#define GAIN_REMOTE_TO_RATE 	-0.00864f

#define OUTPUT_LIMIT 					10.0f //2000
#define RATE_DECREASEMENT 		2.0f
#define RATE_INCREASEMENT 		2.0f
#define LIMIT_RATE   					5.236f
#define ALPHA 								1.0f
#define ZERO_COLL_PWM 				1500
#define YAW_DIRECTION  				-
// #define _AVCS_TUNING_
//void EXTI15_10_IRQHandler(void);






class CONTROL_ALTITUDE:public PID
{
public:
	CONTROL_ALTITUDE(){}
	~CONTROL_ALTITUDE(){}


	int heli_type;//直升机类型
	eMODE control_mode;//控制模式
	int loop_count;//下传数据用的程序循环次数
	int Mau_Loop_count;//手控模式程序循环次数
	int Att_Loop_count;//姿态模式程序循环次数
	int Pos_Loop_count;//定点模式程序循环次数
	int Spe_Loop_count;//
	int manual_servo_width[8];//手控遥控器值，用来采集中位点
	int auto_servo_width[8];//自控状态下的舵机输出值
	int auto_servo_width_temp[3];//无副翼加耦解算的舵机缓存值
	int couple_servo_width[3];//加耦后的舵机输出值
	int output_servo_width[8];//最后输出的舵机输出值
	int amplify;

	int pwm_roll_mid;//roll 中位点
	int pwm_pitch_mid;//pitch 中位点
	int pwm_yaw_mid;//yaw 中位点
	int pwm_coll_mid;//coll 中位点

	int auto_roll_control_mode;
	int auto_pitch_control_mode;
	int auto_yaw_control_mode;
	int auto_coll_control_mode;

	float roll_command;
	float pitch_command;
	float yaw_command;
	float roll_rate_command;
	float pitch_rate_command;
	float yaw_rate_command;
	float roll_command_from_rc;
	float pitch_command_from_rc;
	float yaw_command_from_rc;
	float yaw_rate_command_from_rc;
	float roll_command_from_outloop;
	float pitch_command_from_outloop;
	float yaw_rate_command_from_outloop;
	float roll_rate_feedforward;
	float pitch_rate_feedforward;
	float yaw_rate_feedforward;
	float roll_rate_feedforward_bf;
	float pitch_rate_feedforward_bf;
	float yaw_rate_feedforward_bf;

	float pos_command_ned[3];
	float pos_feedback_ned[3];
	float pos_error_ned[3];
	float vel_command_ned[3];
	float vel_command_ned_last[3];
	float vel_command_ned_error_old[3];
	float vel_intergrate_ned_error[3];
	float vel_ned_ff[3];
	float vel_feedback_ned[3];
	float acc_command_ned[3];
	float acc_command_body[3];
	float acc_command_body_last[3];
	float acc_command_body_error_old[3];
	float acc_command_d_error_old;
	float acc_intergrate_d_error;

	float roll_feedback;
	float pitch_feedback;
	float yaw_feedback;
	float roll_rate_feedback;
	float pitch_rate_feedback;
	float yaw_rate_feedback;

	float angle_error_ef[3];
	float angle_error_bf[3];
	float output[4];//控制量

	//锁尾
	float rate_error;
	float _rate_error;
	float target_rate;
	bool flag_breach_limit;
	float yaw_out;
	float raw_target_rate;
	bool clear_flag;
	bool parameter_flag;
	enum nonLinearController{
		Controller1,
		Controller2,
		Controller3
	};

	void Control_Init(void);
	void Heli_Attitude_control(void);
	void Angle_From_Rc_Or_Outloop(void);
	void Ang_Loop_Feedback(void);
	void Ang_Rate_Loop_Feedback(void);
	float sqrt_controller(float error, float p, float second_ord_lim);
	void update_ef_roll_pitch_angle_and_error(void);
	void Angle_error_ef_2_bf(void);
	void update_rate_bf_targets(void);
	void Angle_Loop(void);
	void Angle_Rate_Loop(void);
	void Attitude_Control_Step(void);
	void Flybarless_H1_DecoupingMatrix(void);
	void Flybarless_CoupingMatrix(void);
	void Flybarless_servos_widths_output(void);
	void Avcs(void);
	void ADRC_Avcs(void);
	void ESO_Avcs1(void);
	void ESO_Avcs2(void);
	void Integral_Init(void);

	PID pid[PID_NUM];

	PID *pidRolRate1=&pid[0],*pidPitRate1=&pid[1],*pidYawRate1=&pid[2];
	PID *pidRol1=&pid[3],*pidPit1=&pid[4],*pidYaw1=&pid[5];
	PID *pidVelX=&pid[6],*pidVelY=&pid[7],*pidVelZ=&pid[8];
	PID *pidX=&pid[9],*pidY=&pid[10],*pidZ=&pid[11];
	PID *pidRolRate1_FF=&pid[12],*pidPitRate1_FF=&pid[13], *pidAcc_z=&pid[14];
	PID *pidTest3 = &pid[15], *pidTest1 = &pid[16], *pidTest2 = &pid[17];



protected:
	PID_msg pid_msg;
	control_transfer_msg control_data;
	RC_command_msg rcCommand;
	RC_Status_msg rc_status_msg;
	sensor_gyro_msg gyro;
	sensor_acc_msg acc;
	sensor_mag_msg mag;
	sensor_acc_filter_msg acc_fil;
	sensor_gyro_filter_msg gyro_filter;
	Control_output_msg control_output;
	Motor_msg motorPWM;
	ekf_cplfil_msg cplfil;
	sensor_baroAlt_msg baroAlt;
	eskf_msg eskf_output;
	ADRC_msg adrc;
};


#endif



#endif /* CONTROL_HPP_ */
